from Node import Node
from point import point
from plotmaze import plot3DBase


class Astar:  # 定义A*算法中的函数
    def __init__(self, maze, startnode, endnode, length, width, height):
        self.openList = []
        self.closeList = []  # 维护的open与close表
        self.maze = maze  # 地图信息，记录不可行点
        self.startNode = startnode
        self.endNode = endnode
        self.currNode = startnode  # 全局起始终点节点信息，以及更新的当前节点
        self.path = []  # 记录路径
        self.length = length
        self.width = width
        self.height = height

    def minFvalue(self):  # 计算openlist中的最小f节点
        minNode = self.openList[0]
        for tempNode in self.openList:
            if tempNode.f < minNode.f:
                minNode = tempNode
        return minNode

    def isOpen(self, thisNode):  # 判断该节点是否在openlist中
        for tempNode in self.openList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y and tempNode.point.z == thisNode.point.z:
                return True
        return False

    def isclose(self, thisNode):  # 判断该节点是否在closelist中
        for tempNode in self.closeList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y and tempNode.point.z == thisNode.point.z:
                return True
        return False

    def isLaststep(self):  # 判断openlist中是否含有end点，判断是否为最后一步
        for tempNode in self.openList:
            if tempNode.point.x == self.endNode.point.x and tempNode.point.y == self.endNode.point.y and tempNode.point.z == self.endNode.point.z:
                return True
        return False

    def getNodeinOL(self, thisNode):  # 通过坐标索引open表中的节点和信息
        for tempNode in self.openList:
            if thisNode.point.x == tempNode.point.x and thisNode.point.y == tempNode.point.y and thisNode.point.z == tempNode.point.z:
                return tempNode
        return None

    def isAccess(self, thisPoint):  # 判断point是否可通行
        if thisPoint.x < 0 or thisPoint.y < 0 or thisPoint.z < 0:  ##低于下界
            return False
        if thisPoint.x >= self.length or thisPoint.y >= self.width or thisPoint.z >= self.height:  ##高于上界
            return False
        for obPoint in self.maze:  # 处于障碍物点
            if obPoint[0] + 0.5 == thisPoint.x and obPoint[1] + 0.5 == thisPoint.y and obPoint[2] >= thisPoint.z:
                return False
        return True

    def searchNode(self, thisNode):
        gValue = 1
        if self.isAccess(thisNode.point) == False:  # 不可行点
            return
        if self.isclose(thisNode) == True:  # 已经遍历过该点
            return
        if self.isOpen(thisNode) == False:  # 不在open表中，则进行操作
            thisNode.getGvalue(gValue + self.currNode.g)  # 更新该节点g
            thisNode.getManhattendis(self.endNode)  # 更新该节点h
            thisNode.ancient = self.currNode  # 更新该节点父亲节点
            self.openList.append(thisNode)  # 加入open表中
        else:  # 若已经在open表中
            tempNode = self.getNodeinOL(thisNode)  # 把open表中的该点找出来
            if self.currNode.g + gValue < tempNode.g:  # 更新g、以及父亲节点
                tempNode.g = self.currNode.g + gValue
                tempNode.ancient = self.currNode
        return

    def searchWay(self):  # 定义四个方向，分别搜索
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y, self.currNode.point.z - 1)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y, self.currNode.point.z - 1)))

        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y, self.currNode.point.z + 1)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y, self.currNode.point.z + 1)))

        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z + 1)):
            self.searchNode(Node(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z)))

        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z - 1)):
            self.searchNode(Node(point(self.currNode.point.x + 1, self.currNode.point.y, self.currNode.point.z - 1)))
        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z - 1)):
            self.searchNode(
                Node(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z - 1)))

        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z - 1)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y + 1, self.currNode.point.z - 1)))
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z - 1)):
            self.searchNode(
                Node(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z - 1)))

        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z + 1)):
            self.searchNode(Node(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z + 1)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z + 1)):
            self.searchNode(
                Node(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z + 1)))

        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z + 1)):
            self.searchNode(
                Node(point(self.currNode.point.x - 1, self.currNode.point.y + 1, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z + 1)):
            self.searchNode(
                Node(point(self.currNode.point.x + 1, self.currNode.point.y - 1, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z - 1)):
            self.searchNode(
                Node(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z - 1)))

        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z)):
            self.searchNode(Node(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z)))
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z - 1)):
            self.searchNode(Node(point(self.currNode.point.x - 1, self.currNode.point.y, self.currNode.point.z - 1)))
        if self.isAccess(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z - 1)):
            self.searchNode(Node(point(self.currNode.point.x, self.currNode.point.y - 1, self.currNode.point.z - 1)))

        if self.isAccess(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z + 1)):
            self.searchNode(
                Node(point(self.currNode.point.x + 1, self.currNode.point.y + 1, self.currNode.point.z + 1)))
        if self.isAccess(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z - 1)):
            self.searchNode(
                Node(point(self.currNode.point.x - 1, self.currNode.point.y - 1, self.currNode.point.z - 1)))

        return

    def run(self):
        self.startNode.getManhattendis(self.endNode)
        self.startNode.getGvalue(0)  # 初始化起点的g与h以及f

        self.openList.append(self.startNode)
        self.currNode = self.minFvalue()  # 将初始点加入open表中，并作为currNode
        self.closeList.append(self.currNode)  # 将该节点放入close表中

        self.searchWay()  # 探索周围节点
        self.openList.remove(self.currNode)  # 探索完毕，从open中删除该节点
        while True:  ##开始遍历
            self.currNode = self.minFvalue()  # 从open表中选currNode进行操作
            self.closeList.append(self.currNode)
            self.searchWay()
            self.openList.remove(self.currNode)

            if self.isLaststep():  # 下一步可以到达终点(在open表中说明已经建立父节点关系)
                tempNode = self.getNodeinOL(self.endNode)
                print("at last")
                while True:
                    self.path.append(tempNode)
                    print(tempNode.point.z)
                    if tempNode.ancient != None:
                        tempNode = tempNode.ancient
                    else:
                        return self.path

            elif len(self.openList) == 0:
                print("self.openList")
                return False
